/*
* $Id: mac.c,v 1.36 2008/03/06 07:43:25 sunny Exp $
*/
/*******************************************************************************

       UBEC (Uniband Electronic Corp.)

       Project: U-NET01, Ubiquitous network platform

       File: mac.c

       Version: 0.3.0

       Usage: Mac subfunction

       Platform: U-NET01 DK with Keil 8051 C compiler

       Reference:

               Silicon Laboratories: C8051F124, C8051F340

               UBEC: UZ2400, UZ2410

       Note :

               Copyright (C) 2007 Uniband Electronic Corporation, All rights reserved

********************************************************************************/

#include "unet.h"

UINT8 unet_tx_bcn(void)
{
	MAC_BCN_PKT *BcnPtr;

	TRACE_puts("unet_tx_bcn");

	BcnPtr = (MAC_BCN_PKT *) RF_TX_BUFF;

	BcnPtr->Header.val = 0x8000;
	BcnPtr->SrcAddr = UnetSys.NwkAddr;
	BcnPtr->SrcPID = UnetSys.JoinPID;
	BcnPtr->Seqnum = UnetSys.MacBSN;
	UnetSys.MacBSN++;
	BcnPtr->GTSF = 0;
	BcnPtr->PASF = 0;

	UnetSys.BcnPayload.ChildCnt = UnetSys.JoinRouterCnt + UnetSys.JoinEndevCnt;
	UnetSys.BcnPayload.Channel = UnetSys.Channel;
	MSG_Copy(&BcnPtr->PAYLOAD,  &UnetSys.BcnPayload, sizeof(MAC_BP));

#ifndef I_Am_Coordinator
	//if the table is full or permitjoin disable, do not tx bcn.
//	if((UnetSys.JoinRouterCnt + UnetSys.JoinEndevCnt >= UnetSys.MaxChild) || UnetSys.Flag.PermitJoin == 0)
//		return 0;
#endif

	if(UnetSys.Flag.PanCoordinator || UnetSys.Flag.Router) {
		BcnPtr->SFS = 0x000f;

		if(UnetSys.Flag.PanCoordinator) {
			BcnPtr->SFS |= 0x4000;
		}

		if(UnetSys.Flag.PermitJoin
			&& (UnetSys.JoinRouterCnt < UnetSys.MaxRouter)
			&& ((UnetSys.JoinRouterCnt + UnetSys.JoinEndevCnt) < UnetSys.MaxChild))
		{
			BcnPtr->SFS |= 0x8000;
		}

		return UzTx(RF_TX_BUFF, sizeof(MAC_BCN_PKT), MT_BCN_IND, 0xff);
	}

	return RETRY_EXCEED;
}


UINT8 unet_tx_bcn_req(void){
	MCP_BCN_REQ *CmdPtr;

	TRACE_puts("unet_tx_bcn_req");

	CmdPtr = (MCP_BCN_REQ *) RF_TX_BUFF;

	CmdPtr->Header.val = 0x0803;
	CmdPtr->DstAddr = 0xffff;
	CmdPtr->DstPID = 0xffff;
	CmdPtr->Seqnum = UnetSys.MacDSN;
	UnetSys.MacDSN++;
	CmdPtr->COMM_ID = MT_BEACON_REQ;

//	if(UzTx(RF_TX_BUFF, sizeof(MCP_BCN_REQ), CmdPtr->COMM_ID, 0xff))
//		return SUCCESS;

//	return FAILURE;
	return UzTx(RF_TX_BUFF, sizeof(MCP_BCN_REQ), CmdPtr->COMM_ID, 0xff);
}

#ifndef I_Am_Coordinator
static UINT8 better_parent_node(MAC_BCN_PKT *BcnPkt, UINT8 PktLength)
{
	UINT8 rssi;

//	if(BcnPkt->SrcPID != UnetSys.JoinPID)		// 1. match target PID: filtered by chip
//		return(0);

	if(!(BcnPkt->SFS & 0x8000)) {			// 2. allow join
		// NOT allow join ==> add to denier list ==> try it later if no suitable node found

	#if USE_DENIER_LIST
		TRACE_printf(("better_parent_node: add denier 0x%04X", BcnPkt->SrcAddr));
		if(insert_node((void *)&UnetSys.DenierTail, sizeof(DENIER_LIST_T)) == NULL) {
			TRACE_puts("better_parent_node: insert_node() FAILURE");
		}
		else {
			UnetSys.DenierTail->PID = BcnPkt->SrcPID;
			UnetSys.DenierTail->Addr = BcnPkt->SrcAddr;
			UnetSys.DenierTail->Channel = UnetSys.Channel;
			UnetSys.DenierTail->MaxDeny = MAX_DENY_COUNT;
		}
	#endif // #if USE_DENIER_LIST

		return(0);
	}

	if(BcnPkt->PAYLOAD.DevDep >= UnetSys.MaxDepth)	// 3. under max. depth
		return(0);

	switch(PktLength) {
	case 16:	// old version beacon without 'ChildCnt'.
		BcnPkt->PAYLOAD.ChildCnt = beacon.ChildCnt;	// the first byte of FCS is destoried

	case 17:	// old version beacon without 'Channel'.
		BcnPkt->PAYLOAD.Channel = UnetSys.Channel;	// the second byte of FCS is destoried
		break;

	default:
		if(PktLength < 18)
			return(0);

		// 2007/11/2 14:37, Sunny: validate the payload to filter out un-expected/fake beacon packet
		if(UnetSys.Channel != BcnPkt->PAYLOAD.Channel) {
			TRACE_printf(("Unexpected beacon packet from channel %u", BcnPkt->PAYLOAD.Channel));
			return(0);
	        }
	}

	rssi = *((UINT8 *)BcnPkt + PktLength /* sizeof(MAC_BCN_PKT) + 2 */ + 1); // 2 + 1 = FCS + LQI
	TRACE_printf(("DevDep=%d (%d), ChildCnt=%d (%d), RSSI=%02X (%02X)",
		(UINT16)(BcnPkt->PAYLOAD.DevDep), (UINT16)(beacon.DevDep),
		(UINT16)(BcnPkt->PAYLOAD.ChildCnt), (UINT16)(beacon.ChildCnt),
		(UINT16)(rssi), (UINT16)(beacon.LinkQuality)));

	if(BcnPkt->PAYLOAD.DevDep == beacon.DevDep) {	// 4a. same hop count
		TRACE_puts("same hop count");
		if(BcnPkt->PAYLOAD.ChildCnt < beacon.ChildCnt){	// less children
			TRACE_puts("less children");
			return(1);
		}
		if(rssi > beacon.LinkQuality) {			// better signal
			TRACE_puts("better signal");
			return(1);
		}
	}
	else if(BcnPkt->PAYLOAD.DevDep < beacon.DevDep){ // 4b. close to coordinator
		TRACE_puts("close to coordinator");
		if(rssi > (beacon.LinkQuality / 4)) {	//	and signal not too worse
			TRACE_puts("signal not too worse");
			return(1);
		}
	}
	else {						// 4c. away from coordinator
		TRACE_puts("away from coordinator");
		if(rssi > ((UINT16)(beacon.LinkQuality) * 8)) {	//	but signal is much better
			TRACE_puts("signal is much better");
			return(1);
		}
	}

	return(0);
}
#endif // #ifndef I_Am_Coordinator

UINT8 unet_rx_bcn_ind(MAC_BCN_PKT *BcnPkt, UINT8 PktLength)
{
#if USE_DENIER_LIST
	DENIER_LIST_T *denier;
#endif

	TRACE_puts("unet_rx_bcn_ind");

#ifdef I_Am_Coordinator
	if(BcnPkt->SrcPID == UnetSys.JoinPID /* && BcnPkt->SrcPID == UNET_COORD_ADDRESS */) {
		UnetSys.Flag.PanIdConflict = 1;
	}
#else
    #if !USE_BEACON_LIST
	if(!((UnetSys.RejoinParent == 0xffff) || (BcnPkt->SrcAddr == UnetSys.RejoinParent))) {
		TRACE_printf(("unet_rx_bcn_ind: (0x%04X) not RejoinParent (0x%04X)", BcnPkt->SrcAddr, UnetSys.RejoinParent));
		return FAILURE;
	}

      #if USE_DENIER_LIST
	if((denier = UnetSys.DenierTail) != NULL) {
	    do {
		denier = denier->Next;
		if((UnetSys.Channel == denier->Channel) && (BcnPkt->SrcPID == denier->PID) && (BcnPkt->SrcAddr == denier->Addr)) {
			TRACE_printf(("unet_rx_bcn_ind: denier 0x%04X", denier->Addr));
			if(denier->MaxDeny --)		// limited deny, try it if no suitable node found: prevent nobody join into the network
				return FAILURE;

			TRACE_printf(("unet_rx_bcn_ind: remove denier 0x%04X", denier->Addr));
			delete_node((void *)&UnetSys.DenierTail, (void *)&denier);
			BcnPkt->SFS |= 0x8000;		// fake permission, take a chance to try
			break;
		}
	    } while(denier != UnetSys.DenierTail);
	}
      #endif // #if USE_DENIER_LIST

	if(better_parent_node(BcnPkt, PktLength)) {
		beacon.PID = BcnPkt->SrcPID;
		beacon.Addr = BcnPkt->SrcAddr;
		beacon.Channel = UnetSys.Channel;
		beacon.SfSpec = BcnPkt->SFS;
		beacon.DevDep = BcnPkt->PAYLOAD.DevDep;
		beacon.ChildCnt = BcnPkt->PAYLOAD.ChildCnt;
		beacon.LinkQuality = *((UINT8 *)BcnPkt + PktLength /* sizeof(MAC_BCN_PKT) + 2 */ + 1); // 2 + 1 = FCS + LQI
		UnetSys.Flag.FindNetwork = 1;
		TRACE_printf(("unet_rx_bcn_ind: better parent node 0x%04X", beacon.Addr));
	}

    #else
	if(insert_node((void *)&UnetSys.BcnTail, sizeof(struct _beacon_list_)) == NULL) {
		TRACE_puts("unet_rx_bcn_ind: insert_node() FAILURE");
		return FAILURE;
	}

	UnetSys.BcnTail->PID = BcnPkt->SrcPID;
	UnetSys.BcnTail->Addr = BcnPkt->SrcAddr;
	UnetSys.BcnTail->Channel = UnetSys.Channel;
	UnetSys.BcnTail->SfSpec = BcnPkt->SFS;
	UnetSys.BcnTail->DevDep = BcnPkt->PAYLOAD.DevDep;
	UnetSys.BcnTail->ChildCnt = BcnPkt->PAYLOAD.ChildCnt;
	UnetSys.BcnTail->LinkQuality = *((UINT8 *)BcnPkt + PktLength /* sizeof(MAC_BCN_PKT) + 2 */ + 1); // 2 + 1 = FCS + LQI
    #endif // #if !USE_BEACON_LIST
#endif // #ifdef I_Am_Coordinator

//	TRACE_puts("unet_rx_bcn_ind: SUCCESS");
	return SUCCESS;
}


UINT8 unet_tx_asso_req(void){
	MCP_ASSO_REQ *AssoPtr;

	TRACE_printf(("unet_tx_asso_req to 0x%04X", UnetSys.JoinParent));

	AssoPtr = (MCP_ASSO_REQ *) RF_TX_BUFF;

	//AssoPtr->Header.val = 0xc823;
	AssoPtr->Header.val = 0xc803;//disable ACK
	AssoPtr->SrcPID = UnetSys.JoinPID;

	MSG_Copy(AssoPtr->SrcAddr, MAC_ADDRESS, 8);

	AssoPtr->DstAddr = UnetSys.JoinParent;
	AssoPtr->DstPID = UnetSys.JoinPID;
	AssoPtr->Seqnum = UnetSys.MacDSN;
	UnetSys.MacDSN++;
	AssoPtr->COMM_ID = MT_ASSO_REQ;
	AssoPtr->Cap = 0xca;

//	if(UzTx(RF_TX_BUFF, sizeof(MCP_ASSO_REQ), AssoPtr->COMM_ID, 0xff))
//		return SUCCESS;
	return UzTx(RF_TX_BUFF, sizeof(MCP_ASSO_REQ), AssoPtr->COMM_ID, 0xff);

//	return RETRY_EXCEED;
}

UINT8 unet_rx_asso_req(MCP_ASSO_REQ *MsgPtr){

	struct _child_list_ *ChiPtr;
//	UINT8 i;
	MCP_ASSO_RSP *RsPtr;

	TRACE_puts("unet_rx_asso_req");

	RsPtr = (MCP_ASSO_RSP *) RF_TX_BUFF;

	//Check associated device if already exist
	if((ChiPtr = UnetSys.ChildTail) != NULL) {
	    do {
		ChiPtr = ChiPtr->Next;
		if(memcmp(ChiPtr->MacAddr, MsgPtr->SrcAddr, 8) == 0) {
			RsPtr->ShortAddr = ChiPtr->NetworkAddress;
			RsPtr->AssoStatus = ASSOCIATION_SUCCESS;

			if(ChiPtr->Available == 0)
				ChiPtr->Available = 1;

#if DEBUG_MODE
		    {
			UINT8 i;
			DBG_puts("\rExistNode: MacAddr=");
			for(i=8; i--; ){
				DBG_printf(("%02X%c", (UINT16)MsgPtr->SrcAddr[i], i ? ':' : ','));
			}
			DBG_printf((" NwkAddr=%u (0x%04x)", RsPtr->ShortAddr, RsPtr->ShortAddr));
		    }
#endif // #if DEBUG_MODE

			TRACE_puts("unet_rx_asso_req: TX_ASSO_RSP_LABEL");
			goto TX_ASSO_RSP_LABEL;
		}
	    } while(ChiPtr != UnetSys.ChildTail);
	}

	//Check CAPACITY
	if(MsgPtr->Cap & 0x02){ // Router
		if((UnetSys.JoinRouterCnt + UnetSys.JoinEndevCnt) < UnetSys.MaxChild && UnetSys.JoinRouterCnt < UnetSys.MaxRouter) {
			UnetSys.JoinRouterCnt++;
		}
		else {
#ifdef TX_ASO_RSP_FAILURE
			RsPtr->AssoStatus = PAN_AT_CAPACITY;
			TRACE_puts("unet_rx_asso_req: TX_ASSO_RSP_LABEL");
			goto TX_ASSO_RSP_LABEL;
#else
			return CHILD_FULL;
#endif
		}
	}
	else{ // End device
		if((UnetSys.JoinRouterCnt + UnetSys.JoinEndevCnt) < UnetSys.MaxChild) {
			UnetSys.JoinEndevCnt++;
		}
		else {
#ifdef TX_ASO_RSP_FAILURE
			RsPtr->AssoStatus = PAN_AT_CAPACITY;
			TRACE_puts("unet_rx_asso_req: TX_ASSO_RSP_LABEL");
			goto TX_ASSO_RSP_LABEL;
#else
			return CHILD_FULL;
#endif
		}
	}

	//Allocate new entry and update information
	if(insert_node((void *)&UnetSys.ChildTail, sizeof(struct _child_list_)) == NULL){
#ifdef TX_ASO_RSP_FAILURE
		RsPtr->AssoStatus = MEMORY_FULL;
		TRACE_puts("unet_rx_asso_req: TX_ASSO_RSP_LABEL");
		goto TX_ASSO_RSP_LABEL;
#else
		return FAILURE;
#endif
	}

	//Allocate address
	if(AddrRecycleFlg){
		RecycleIndex-=1;

		if(RecycleAddress[RecycleIndex] == 0xffff) {
			TRACE_puts("unet_rx_asso_req: NORMAL_ALLOCATE");
			goto NORMAL_ALLOCATE;
		}
		RsPtr->ShortAddr = RecycleAddress[RecycleIndex];
		RecycleAddress[RecycleIndex] = 0xffff;
		RsPtr->AssoStatus = ASSOCIATION_SUCCESS;

		if(RecycleIndex == 0)
			AddrRecycleFlg = 0;
	}
	else{
   NORMAL_ALLOCATE:
		TRACE_puts("unet_rx_asso_req: NORMAL_ALLOCATE");

		RsPtr->ShortAddr = UnetSys.NextAddr;
		RsPtr->AssoStatus = ASSOCIATION_SUCCESS;
		UnetSys.NextAddr += UnetSys.AddrInc;

		TRACE_printf(("UnetSys.NextAddr=%u, UnetSys.AddrInc=%u", UnetSys.NextAddr, UnetSys.AddrInc));
	}

	//Update child information
	UnetSys.ChildTail->Cap = MsgPtr->Cap;
	UnetSys.ChildTail->NetworkAddress = RsPtr->ShortAddr;
	UnetSys.ChildTail->Available = 1;
	UnetSys.ChildTail->rx_polling_flg = 0;
	UnetSys.ChildTail->rx_polling_switch = 0;
	UnetSys.ChildTail->tx_failure = 0;

#if DEBUG_MODE
   	{
		UINT8 i;
		DBG_puts("\rNewNode: MacAddr=");
		for(i=8; i--; ){
			DBG_printf(("%02X%c", (UINT16)MsgPtr->SrcAddr[i], i ? ':' : ','));
		}
		DBG_printf((" NwkAddr=%u (0x%04x)", RsPtr->ShortAddr, RsPtr->ShortAddr));
   	}
#endif // #if DEBUG_MODE
	MSG_Copy(UnetSys.ChildTail->MacAddr, MsgPtr->SrcAddr, 8);

TX_ASSO_RSP_LABEL:
	TRACE_puts("unet_rx_asso_req: TX_ASSO_RSP_LABEL");

#ifdef TX_ASO_RSP_FAILURE
	if(RsPtr->AssoStatus != ASSOCIATION_SUCCESS){
		RsPtr->ShortAddr = 0xffff;
	}
#endif

	// processing the rsp packet
	RsPtr->Header.val = 0x8c23;
	RsPtr->Seqnum = UnetSys.MacDSN;
	UnetSys.MacDSN++;
	RsPtr->SrcAddr = UnetSys.NwkAddr;
	RsPtr->SrcPID = UnetSys.JoinPID;
	RsPtr->DstPID = UnetSys.JoinPID;
	RsPtr->COMM_ID = MT_ASSO_RSP;

	RsPtr->MaxChild = UnetSys.MaxChild;
	RsPtr->MaxRouter = UnetSys.MaxRouter;
	RsPtr->MaxDepth = UnetSys.MaxDepth;

	MSG_Copy(RsPtr->DstAddr, MsgPtr->SrcAddr, 8);

	if(UzTx(RF_TX_BUFF, sizeof(MCP_ASSO_RSP), RsPtr->COMM_ID, 0xff)){
		//prepare the information of the device
#ifdef TX_ASO_RSP_FAILURE
		if(RsPtr->ShortAddr == 0xffff) {
			TRACE_puts("unet_rx_asso_req: ASSO_FALSE_LABEL");
			goto ASSO_FALSE_LABEL;
		}
#endif
		SWAPINT16(&(RsPtr->ShortAddr));

		if(UnetSys.NwkAddr != UNET_COORD_ADDRESS){
//			TxUnetJoinReport(MsgPtr->SrcAddr, RsPtr->ShortAddr);
			reset_timer(reqtable_tm);
		}
		else{
		//router join the coordinator
#ifdef I_Am_Coordinator
			struct _unet_children_table_ *ChiPtr;

			if((ChiPtr = UnetSys.ChildTableTail) != NULL) {
			    do {
				ChiPtr = ChiPtr->Next;
				if(memcmp(ChiPtr->MacAddress, MsgPtr->SrcAddr, 8) == 0){
					if(ChiPtr->NwkAddress == RsPtr->ShortAddr){
						TRACE_puts("unet_rx_asso_req: UNET_RX_ASSO_REQ_LABEL");
						goto UNET_RX_ASSO_REQ_LABEL;
					}
					ChiPtr->NwkAddress = RsPtr->ShortAddr;
					ChiPtr->ParentAddress = UnetSys.NwkAddr;
					TRACE_puts("unet_rx_asso_req: UNET_RX_ASSO_REQ_LABEL");
					goto UNET_RX_ASSO_REQ_LABEL;
				}
			    } while(ChiPtr != UnetSys.ChildTableTail);
			}

			if(insert_node((void *)&UnetSys.ChildTableTail, sizeof( struct _unet_children_table_)) == NULL) {
				TRACE_puts("unet_rx_asso_req: MEMORY_FULL");
				return FAILURE;
			}

			UnetSys.ChildTableTail->NwkAddress = UnetSys.ChildTail->NetworkAddress;
			UnetSys.ChildTableTail->ParentAddress = UnetSys.NwkAddr;

			MSG_Copy(UnetSys.ChildTableTail->MacAddress, MsgPtr->SrcAddr, 8);

//			UNET_GET_TABLE();
			reset_timer(gettable_tm);
#endif
		}

#ifdef I_Am_Coordinator
UNET_RX_ASSO_REQ_LABEL:
		TRACE_puts("unet_rx_asso_req: UNET_RX_ASSO_REQ_LABEL");
		return SUCCESS;
#endif

	}
	else {	// 2007/7/11 11:26, Sunny: in case we can't send the asso. resp. to target node.
TRACE_TAG();		// TODO: remove this child from list
		SWAPINT16(&(RsPtr->ShortAddr));
		if((ChiPtr = UnetSys.ChildTail) != NULL && RsPtr->ShortAddr != 0xffff) {
		    do {
			ChiPtr = ChiPtr->Next;
			if(ChiPtr->NetworkAddress == RsPtr->ShortAddr) {
				delete_node((void *)&UnetSys.ChildTail, (void *)&ChiPtr);
				RecycleAddress[RecycleIndex] = RsPtr->ShortAddr;
				RecycleIndex+=1;
				AddrRecycleFlg = 1;
				if(ChiPtr->Cap & 0x02)	// Router
					UnetSys.JoinRouterCnt--;
				else	// End device
					UnetSys.JoinEndevCnt--;
				break;
			}
		    } while(ChiPtr != UnetSys.ChildTail);
		}
	}
#ifdef TX_ASO_RSP_FAILURE
ASSO_FALSE_LABEL:
#endif
	TRACE_puts("unet_rx_asso_req: ASSO_FALSE_LABEL");
	return RETRY_EXCEED;
}


void unet_rx_asso_rsp(MCP_ASSO_RSP *RsPtr){

	UnetSys.Flag.RecvAssoRsp = 1;

	TRACE_printf(("unet_rx_asso_rsp: Join PANid: %04x, Join Status: %x, Parent: %04x, NwkAddr: %04x", RsPtr->SrcPID, (UINT16)RsPtr->AssoStatus, RsPtr->SrcAddr, RsPtr->ShortAddr));

	if(RsPtr->AssoStatus == ASSOCIATION_SUCCESS && RsPtr->SrcAddr == UnetSys.JoinParent){
		UnetSys.Flag.AssoSuccess = 1;
		UnetSys.NwkAddr = RsPtr->ShortAddr;

		UnetSys.MaxChild = RsPtr->MaxChild;
		UnetSys.MaxRouter = RsPtr->MaxRouter;
		UnetSys.MaxDepth = RsPtr->MaxDepth;

		DBG_puts(", OK");
	}
	else {
		DBG_puts(", NG");
	}

	return;
}

UINT8 unet_rx_data_ind(UNET_HEADER **MsgPtr, UINT8 Length){
	struct _broadcast_list_ *BroPtr;

//	UINT8 i;

	TRACE_printf(("unet_rx_data: MsgPtr=%p, DstAddr=%04X, SrcAddr=%04X", *MsgPtr, (*MsgPtr)->DstAddr, (*MsgPtr)->SrcAddr));

	//Check destination
	if((*MsgPtr)->DstAddr == 0xffff){ // if broadcast
		UnetSys.Flag.Broadcast = 1;
		if((*MsgPtr)->SrcAddr == UnetSys.NwkAddr) { // This broadcast issued by devself.
			TRACE_puts("unet_rx_data: DISCARD_FRAME");
			return DISCARD_FRAME;
		}

		if((BroPtr = UnetSys.BroTail) != NULL) {
		    do {
			BroPtr = BroPtr->Next;
			if(BroPtr->Address == (*MsgPtr)->SrcAddr){
				//if((*MsgPtr)->MHR.Seqnum > BroPtr->Sequence || ((*MsgPtr)->MHR.Seqnum < 5 && BroPtr->Sequence >250)){ // Compare sequence number
				if((*MsgPtr)->MHR.Seqnum != BroPtr->Sequence){ // Compare sequence number
					BroPtr->Sequence = (*MsgPtr)->MHR.Seqnum;
					TRACE_puts("unet_rx_data: SEND_TO_QUEUE_LABEL");
					goto SEND_TO_QUEUE_LABEL;
				}
				TRACE_puts("unet_rx_data: DISCARD_FRAME");
				return DISCARD_FRAME;
			}
		    } while(BroPtr != UnetSys.BroTail);
		}

		if(insert_node((void *)&UnetSys.BroTail, sizeof(struct _broadcast_list_)) == NULL) { // Add new entry
			TRACE_puts("unet_rx_data: MEMORY_FULL");
			return FAILURE;
		}

		UnetSys.BroTail->Address = (*MsgPtr)->SrcAddr;
		UnetSys.BroTail->Sequence = (*MsgPtr)->MHR.Seqnum;

		TRACE_puts("unet_rx_data: SEND_TO_QUEUE_LABEL");
		goto SEND_TO_QUEUE_LABEL;

	}else if((*MsgPtr)->DstAddr == UnetSys.NwkAddr){ // if devself
		UnetSys.Flag.GP0 = 1; // Set flag, indicate it will do only SEND_TO_QUEUE
		TRACE_puts("unet_rx_data: SEND_TO_QUEUE_LABEL");
		goto SEND_TO_QUEUE_LABEL;
	}else{ // Both not , routing
		if(((*MsgPtr)->MHR.DstAddr = unet_tree_routing((*MsgPtr)->DstAddr)) == (UINT16)(-1)) { //Check the destination if allready link to devself by self-healing
			TRACE_puts("unet_rx_data: DISCARD_FRAME");
			return DISCARD_FRAME;
		}

		TRACE_puts("unet_rx_data: TX_DATA_PKT_LABEL");
		goto TX_DATA_PKT_LABEL;
	}

SEND_TO_QUEUE_LABEL:
	TRACE_puts("unet_rx_data: SEND_TO_QUEUE_LABEL");

	if(/*UnetSys.DataCnt < 5 && */ (*MsgPtr)->UnetFrmType == UNET_DATA){ // Max Data Packets were buffered.
		if(insert_node((void *)&UnetSys.DataTail, sizeof(struct _data_list_)) == NULL) { // Add new entry
			TRACE_puts("unet_rx_data: MEMORY_FULL");
			return FAILURE;
		}

		UnetSys.DataTail->Length = Length - sizeof(UNET_HEADER) -2; // 2:FCS

		// 2007/8/8 10:39, Sunny: move message from DataQueue[] to UnetSys.DataTail list.
		UnetSys.DataTail->Msg = (UINT8 *)(*MsgPtr);
		if((*MsgPtr)->DstAddr == 0xffff) { // if broadcast, make a copy for sending
			*MsgPtr = malloc(UnetSys.DataTail->Length + sizeof(UNET_HEADER));
			memcpy(*MsgPtr, UnetSys.DataTail->Msg, UnetSys.DataTail->Length + sizeof(UNET_HEADER));
		}
		else
			*MsgPtr = NULL;
	//	if((UnetSys.DataTail->Msg = (UINT8 *) MSG_Alloc(UnetSys.DataTail->Length)) == NULL){
	//		delete_node((void *)&UnetSys.DataTail, (void *)&UnetSys.DataTail);
	//		TRACE_puts("unet_rx_data: FAILURE");
	//		return FAILURE;
	//	}

		UnetSys.DataTail->SrcAddr = (*MsgPtr)->SrcAddr;

	//	MSG_Copy(UnetSys.DataTail->Msg, ((UINT8 *) (*MsgPtr) + sizeof(UNET_HEADER)), UnetSys.DataTail->Length);

	//	UnetSys.DataCnt++;
	}
	else {
		RxUnetFrameParsing(*MsgPtr);
	}

	if(UnetSys.Flag.GP0){	// if devself, indicate it will do only SEND_TO_QUEUE
		UnetSys.Flag.GP0 = 0;
		TRACE_puts("unet_rx_data: SUCCESS: UnetSys.Flag.GP0");
		return SUCCESS;
	}

TX_DATA_PKT_LABEL:
	TRACE_puts("unet_rx_data: TX_DATA_PKT_LABEL");

	if(*MsgPtr && (((*MsgPtr)->Radius -= 1) > 0)){
		(*MsgPtr)->MHR.SrcAddr = UnetSys.NwkAddr;
		TRACE_printf(("unet_rx_data: MsgPtr=%p, DstAddr=%04X, SrcAddr=%04X", *MsgPtr, (*MsgPtr)->DstAddr, (*MsgPtr)->SrcAddr));

		return UzTx((UINT8 *)(*MsgPtr), Length - 2, MT_DATA_IND, UNET_DATA);
	}

	TRACE_puts("unet_rx_data: DISCARD_FRAME");
	return DISCARD_FRAME;
}

#if 0
static void blink_link_led(void *param)
{
	TRACE_TAG();
	reset_timer(*(TIMER **)param);

	if(Is_Link_LED_On()) {
		Link_LED_Off();
	}
	else {
		Link_LED_On();
	}
}
#endif

void unet_active_scan(void)
{
	BOOL AUTO;
	UINT8 TempChannel, i, j;
//	TIMER *led_tm;

	TRACE_puts("unet_active_scan");

	AUTO = UnetSys.Flag.AutoChannel = (UnetSys.Channel == 0xff);

	if(UnetSys.Flag.AutoPID = (UnetSys.JoinPID == 0xffff)) {
	#ifdef I_Am_Coordinator
		UnetSys.JoinPID = rand() % 0x3fff;
	#endif // #ifdef I_Am_Coordinator
	}

#if DEBUG_MODE
	DBG_puts("\rChannel = ");
	if(UnetSys.Flag.AutoChannel)
		puts("Auto");
	else
		print_int(UnetSys.Channel);

	puts(", PAN ID = ");
	if(UnetSys.Flag.AutoPID)
		puts("Auto");
	else {
		puts("0x");
		print_hex(UnetSys.JoinPID >> 8);
		print_hex(UnetSys.JoinPID & 0xff);
	}
#endif // #if DEBUG_MODE

#ifdef I_Am_Coordinator
	if(AUTO) {
		UINT16 rssi_min = 0xffff;
		UINT16 rssi;

		AUTO = 0;

		for(i=11; i<=26; i++) {		// Scan all channel
		//	Link_LED_Toggle();
			(i & 1) ? Link_LED_On() : Link_LED_Off();

			UzSetChannel(i); // set channel
			for(rssi = 0, j = 20; j--;) {
				WaitMS(50);
				rssi += UzReadRSSI();
			}

			TRACE_printf(("background rssi=%d", rssi));
			if(rssi <= rssi_min) {
				rssi_min = rssi;
				UnetSys.Channel = i;
				DBG_printf((" ==> cleaner channel: %d", (UINT16) (i)));
			}
		}
	}
#endif // #ifdef I_Am_Coordinator

	TempChannel = UnetSys.Channel;

	UzSetPanId(UnetSys.JoinPID);	// set filter of PANID

	UzRxOnlyBeacon();

//	led_tm = setup_timer(500, false, blink_link_led, &led_tm);
	for(i=11; i<=26; i++) {		// Scan all channel
		TIMER *tm;

		if(!(AUTO || (i == TempChannel)))
			continue;	// if not AUTO, skip to target channel

		UzSetChannel(UnetSys.Channel = i); // set channel
		for(j = 0; j <= 1; ++j) {	// 2007/8/31 16:47, Sunny: listen and found nothing then request
			if(j)
				unet_tx_bcn_req();

			tm = setup_timer(UNET_SCAN_WAIT_TIME + (j ? 0 : (rand() % UNET_SCAN_WAIT_TIME)), true, NULL, NULL);
			while(!is_timer_expired(tm)) {
				unet_proc();	//Process received packets as soon as possible
			}

		#if !USE_BEACON_LIST
			if(!AUTO && UnetSys.Flag.FindNetwork) {
			//	NOP();
				break;
			}
		#else
			if(!AUTO && UnetSys.BcnTail)
				break;
		#endif // #if !USE_BEACON_LIST
		}
	}
//	close_timer(led_tm);
//	Link_LED_Off();

	UzRxAllFrame();
	UnetSys.Channel = TempChannel;
}

UINT8 unet_tx_disasso(UINT8 *DST_MAC, UINT8 Reason, UINT16 NewParent)
{
	MCP_DISASSO_NTF *DisPtr;

	TRACE_puts("unet_tx_disasso");

	DisPtr = (MCP_DISASSO_NTF *) RF_TX_BUFF;

	DisPtr->Header.val = 0x8c23;
	DisPtr->Seqnum = UnetSys.MacDSN;
	UnetSys.MacDSN++;
	DisPtr->SrcAddr = UnetSys.NwkAddr;
	DisPtr->SrcPID = UnetSys.JoinPID;
	DisPtr->DstPID = UnetSys.JoinPID;

	MSG_Copy(DisPtr->DstAddr, DST_MAC, 8);

	DisPtr->COMM_ID = MT_DISASSO_NTF;
	DisPtr->DisassoReason = Reason; // The coordinator or router wishes the device to leave the PAN.
	DisPtr->NewParent = NewParent;

	return UzTx(RF_TX_BUFF, sizeof(MCP_DISASSO_NTF),DisPtr->COMM_ID,0xff);
}

UINT8 unet_rx_disasso(MCP_DISASSO_NTF *CmdPtr){
	struct _child_list_ *ChiPtr;

	TRACE_puts("unet_rx_disasso");

	if(CmdPtr->DisassoReason == 0x2){//The device wants to leave
	//1. clean child table
	//2. report to the coordinator to clean child table
		TRACE_puts("unet_rx_disasso: SUCCESS");
		return SUCCESS;
	}

	if(!UnetSys.Flag.Router && !UnetSys.Flag.PanCoordinator){
		if(CmdPtr->SrcAddr == UnetSys.JoinParent) {
			NetworkReset();
			TRACE_puts("unet_rx_disasso: 1");
			return 1;
		}
	}

	if((UnetSys.JoinRouterCnt + UnetSys.JoinEndevCnt) > 0){//The router has children
	//1. permit join => off
		UnetSys.Flag.PermitJoin = 0;
	//2. reset all child
		if((ChiPtr = UnetSys.ChildTail) != NULL) {
		    do { //disassociate all of my children
			ChiPtr = ChiPtr->Next;
			unet_tx_disasso(ChiPtr->MacAddr, 0x1, 0xffff);
//			WaitMS(100 + rand() % 400);
		   } while(ChiPtr != UnetSys.ChildTail);
		}
	}

	//3. reset myself
	if(CmdPtr->SrcAddr == UnetSys.JoinParent){
		UnetSys.RejoinParent = CmdPtr->NewParent;
		NetworkReset();
		TRACE_puts("unet_rx_disasso: 1");
		return 1;
	}

	TRACE_puts("unet_rx_disasso: DISCARD_FRAME");
	return DISCARD_FRAME;
}
